

#include "Matrix4.h"

#include <cmath>
#include <stdio.h>


// invert, return true on success, false on failure
bool Matrix4::invert()
{
	int i, j, k;
	float t;
	
	Matrix4 temp = *this;

	this->setIdentity();

	for(i = 0; i < 4; ++i)
	{
		// Look for largest element in column
		int swap = i;
		for(j = i + 1; j < 4; ++j)
		{
			if(fabsf(temp[ (j * 4) + i] ) > fabsf(temp[ 5*i ]))
			{
				swap = j;
			}
		}
		
		if(swap != i)
		{
			// Swap rows.
			for(k = 0; k < 4; ++k)
			{
				t = temp[ (i * 4) + k ];
				temp[ (i * 4) + k ] = temp[ (swap * 4) + k ];
				temp[ (swap * 4) + k] = t;
				
				t = (*this)[ (i * 4) + k ];
				(*this)[ (i * 4) + k ] = (*this)[ (swap * 4) + k ];
				(*this)[ (swap * 4) + k ] = t;
			}
		}
		
		//  No non-zero pivot.  The matrix is singular, which means the user gave us a bad matrix.
		if( temp[5*i] == 0.0f )
		{
			printf("getInverse_Matrix4() - zero determinant\n");
			return false;
		}
		
		t = temp[ 5*i ];
		for (k = 0; k < 4; ++k)
		{
			temp[ (i * 4) + k ] /= t;
			(*this)[ (i * 4) + k ] /= t;
		}
		
		for(j = 0; j < 4; j++)
		{
			if(j != i)
			{
				t = temp[ (j * 4) + i ];
				
				for(k = 0; k < 4; k++)
				{
					temp[ (j * 4) + k ] -= temp[ (i * 4) + k ] * t;
					(*this)[ (j * 4) + k ] -= (*this)[ (i * 4) + k ] * t;
				}
			}
		}
	}
	
	return true;
}


void Matrix4::setTranslation(const Vec4& translation)
{
	m_matrix[12] = translation[0];
	m_matrix[13] = translation[1];
	m_matrix[14] = translation[2];
	m_matrix[15] = 1.0f;		
}

Vec4 Matrix4::getTranslation() const
{
	Vec4 t(m_matrix[12], m_matrix[13], m_matrix[14], m_matrix[15]);
	return t;
}


static inline void _setRotation(const Vec4& axis, float sin_angle, float cos_angle, Matrix4& matrixOut)
{
	float x = axis[0];
	float y = axis[1];
	float z = axis[2];
	
	float one_minus_cos = 1.0f - cos_angle;
	
	matrixOut[ 0] = (x * x * one_minus_cos) + cos_angle;
	matrixOut[ 1] = (y * x * one_minus_cos) + (z * sin_angle);
	matrixOut[ 2] = (x * z * one_minus_cos) - (y * sin_angle);
	matrixOut[ 3] = 0.0f;
	matrixOut[ 4] = (x * y * one_minus_cos) - (z * sin_angle);
	matrixOut[ 5] = (y * y * one_minus_cos) + cos_angle;
	matrixOut[ 6] = (y * z * one_minus_cos) + (x * sin_angle);
	matrixOut[ 7] = 0.0f;
	matrixOut[ 8] = (x * z * one_minus_cos) + (y * sin_angle);
	matrixOut[ 9] = (y * z * one_minus_cos) - (x * sin_angle);
	matrixOut[10] = (z * z * one_minus_cos) + cos_angle;
	matrixOut[11] = 0.0f;
	matrixOut[15] = 1.0f;
	
}

void Matrix4::setRotation( const Vec4& axis, float angle )
{
	_setRotation(axis, sin(angle), cos(angle), *this);
}

void Matrix4::setRotation( const Vec4& from, const Vec4& to )
{
	Vec4 axis;
	axis = from % to;
	
	float sin_angle = axis.length3();
	float cos_angle = from * to;
	
	axis.normalizeIfNotZero();
	_setRotation(axis, sin_angle, cos_angle, *this);
}
	
